//
// Created by okgwf on 2021/9/22.
//

#include "LevelOneComplementaryFiltering.h"

//一阶互补滤波

float K1 =0.1; // 对加速度计取值的权重

float dt=0.01;//注意：dt的取值为滤波器采样时间

float anglePitch = 0.0;
float angleRoll = 0.0;

/*angle_ax= atan2(ax,az)*57.3;     //加速度得到的角度

gy=(float)gyo[1]/7510.0;       //陀螺仪得到的角速度

Pitch = yijiehubu(angle_ax,gy);*/

//互补算法只能得到一个倾角，这在平衡车项目中够用了，而在四轴飞行器设计中还需要 Roll 和 Yaw，就需要两个 互补算法

float LevelOneComplementaryPitch(float angle_m, float gyro_m)//采集后计算的角度和角加速度
{
    anglePitch = K1 * angle_m + (1-K1) * (anglePitch + gyro_m * dt);
    return anglePitch;
}

float LevelOneComplementaryRoll(float angle_m, float gyro_m)//采集后计算的角度和角加速度
{
    angleRoll = K1 * angle_m + (1-K1) * (angleRoll + gyro_m * dt);
    return angleRoll;
}